/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef PCLICP_H
#define PCLICP_H
#include <qstring.h>
#include <vector>
#include "GeneralDefine.h"
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/gicp.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>


namespace KDL{
class Frame;
}

//// 定义点云使用的格式：这里用的是XYZRGB
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
/**
 * @brief The PclIcp class icp定位算法类
 */
class PclIcp
{
public:
    PclIcp();

public:
    /**
     * @brief setVehiclePoseEstimation 设置变换初值，使求解更快收敛
     * @param x　，ｍ
     * @param y
     * @param r
     * @return
     */
    int setVehiclePoseEstimation(double x,double y,double r);

    int setMatchCalculateParameter(PclCalculationParameter paraIn);
    int getMatchDebugInfo(PclMatchDebugInfo& infoOut);
    int showPclIcpDebugWindow(bool isShowBaseMap, bool isShowBaseCompareMap,
                              bool isShowLocalMap, bool isShowLocalCompareMap);
    int updatePclIcpDebugWindow();
    int setLaserToVehicleInfo(LaserScanDeviceInfo infoIn);
    /**
     * @brief loadBaseMap
     * @param mapPath
     * @param mPerPixIn
     * @param x_offset ,unit is pix
     * @param y_offset ,unit is pix
     * @param r_offset ,not used
     * @return
     */
    int loadBaseMap(QString mapPath,double mPerPixIn,double x_offset,double y_offset,double r_offset);
    int setBaseMapRegion_m(double originX,double originY, double width,double height);
    /**
     * @brief loadLocalMap
     * @param mPerPixIn
     * @param pointX , unit is m　以laser_link为原点
     * @param pointY, unit is m
     * @return
     */
    int loadLocalMap(std::vector<double> pointX, std::vector<double> pointY);


    /**
     * @brief setLocalMapRegion_cm
     * @param originX 相对于laserscan原点的cm值.localmap的原点不在激光头上
     * @param originY
     * @param width
     * @param height
     * @return
     */
    int setLocalMapRegion_m(double originX,double originY, double width,double height);

    int calculateMatch(double &xOut, double &yOut, double &rOut, int &isCalculateSucess_out,
                       double &maxMatchErrorOut, double &matchScoreOut);

    int showBaseMap();
    int showBaseCompareMap();
    int showLocalMap();
    int showLocalCampareMap();
    int showTransformedMap();
    int showMatchWindow();


private:
    int getVehiclePosition_m(double &xOut,double &yOut,double &rOut,int &isCalculateSucess_out);
    int calculateMatch();
    int calculateLaserToVehicleMatrix();
    int pgmToPointCould_cv(QString imgFilePath);
    int pgmToPointCould(QString imgFilePath);
    int pclTest();

    int calculateMatch_generalized();
    int calculateMatch_interative();
    int calculateMatch_normals();
    int calculateMatch_nonLinear();
    int calculateMatch_joint();

    /**
     * @brief addMsg  , 添加消息反馈
     * @param messageLevel
     * @param componentName
     * @param messageType
     * @param messageCode
     * @param robotId
     * @param parameter1
     * @param parameter2
     * @param parameter3
     * @param parameter4
     */
    void addMsg( int messageLevel,
                 std::string componentName,
                 std::string messageType,
                 int messageCode,
                 int robotId,
                 int parameter1 = 0,
                 int parameter2 = 0,
                 int parameter3 = 0,
                 int parameter4 = 0,
                 QString message= "");



private:


    volatile double vehicle_match_x;//m
    volatile double vehicle_match_y;//m
    volatile double vehicle_match_r;//rad

    volatile int isMatchSucess;
    volatile double maxMatchError;//not used
    volatile double matchScore;
    volatile double calculateConsumeTime;//unit is uS


    pthread_mutex_t matchMutex;
    pthread_mutexattr_t matchAttr;
    LaserScanDeviceInfo laserScanDeviceInfo;
    PclMatchDebugInfo matchDebugInfo;
    PclCalculationParameter calculationParemeter;

    PointCloud::Ptr baseMap;
    PointCloud::Ptr baseCompareMap;
    PointCloud::Ptr laserOriginMap;
    PointCloud::Ptr localMap;
    PointCloud::Ptr localCompareMap;
    PointCloud::Ptr transformedMap;
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
//    pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB>icp;
//    pcl::IterativeClosestPointWithNormals<pcl::PointXYZRGB, pcl::PointXYZRGB>icp;
//    pcl::pcl::IterativeClosestPointNonLinear<pcl::PointXYZRGB, pcl::PointXYZRGB>icp;

    pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
    Eigen::Matrix4f userEstimateVehicleInMapMatrix;//用户估计base_link在ｍap矩阵表示
    Eigen::Matrix4f userEstimateMapInVehicleMatrix;//用户估计ｍap在base_link的矩阵表示
    Eigen::Matrix4f calculateEstimateInMapMatrix_last;//icp　上次，车体估计位置在map中的矩阵表示
    Eigen::Matrix4f calculateEstimateInMapMatrix_new;//icp 本次，车体估计位置在map中的矩阵表示
    Eigen::Matrix4f laserToVehicleMatrix;//激光雷达在车体base_link的矩阵表示
    Eigen::Matrix4f vehicleActualMatrix;//base_link 在 map的矩阵表示

    int calculateTimeOutCount;
    int calculateFailedCount;
    int robotId;
};

#endif // PCLICP_H
